#include <ros/ros.h>
#include <ros/duration.h>
#include <std_msgs/String.h>
#include <sensor_msgs/JointState.h>
#include <sensor_msgs/PointCloud2.h>

#include <iostream>
#include <stdio.h>
#include <deque>
#include <thread>

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl_conversions/pcl_conversions.h>

#include "X20_LegKinematic.hpp"

class X20ContactModule
{
public:
    X20LegKinematic X20Leg;

    ros::NodeHandle nh;
    ros::Subscriber subJointState;              // 订阅X20的关节电机数据话题
    ros::Publisher pubX20ContactPoseCloud;      // 发布X20足端接触位置的点云话题

    std::string jointState_Topic = "/joint_states";             // X20电机数据话题名
    std::string X20ContactFoot_Topic = "/X20ContactFoot";       // X20足端接触点云话题名

    Eigen::Matrix<bool, 1, 4> X20ContactPoseRecord;             // 用于标记已经记录足端位置的标志位
    pcl::PointCloud<pcl::PointXYZI>::Ptr X20ContactPoseCloud;   // 用于记录足端位置的点云

    X20ContactModule()
    {
        X20ContactPoseCloud.reset(new pcl::PointCloud<pcl::PointXYZI>());   // 初始化X20足端接触位置点云

        subJointState = nh.subscribe<sensor_msgs::JointState>(jointState_Topic, 1000, &X20ContactModule::jointState_handle, this,  ros::TransportHints().tcpNoDelay());
        pubX20ContactPoseCloud = nh.advertise<sensor_msgs::PointCloud2>(X20ContactFoot_Topic, 1000);
    }



    /***********************************************************
     * @brief X20关节数据话题的接收回调函数
     * 
     * @param msgIn 
     **********************************************************/
    void jointState_handle(const sensor_msgs::JointState::ConstPtr& msgIn)
    {
        X20Leg.getX20InfoFromMsg(msgIn);

        X20ContactPoseRecordProcess();

        // ROS_INFO("\033[1;31m %f; \033[0m", X20Leg.foot_contact(0));
        // ROS_INFO("\033[1;31m %f; \033[0m", X20Leg.foot_contact(1));
        // ROS_INFO("\033[1;31m %f; \033[0m", X20Leg.foot_contact(2));
        // ROS_INFO("\033[1;31m %f; \033[0m", X20Leg.foot_contact(3));
        if (X20Leg.foot_contact(0) > 0)
            ROS_INFO("\033[1;31m %f; \033[0m", X20Leg.foot_pos_imu(2, 0));
        ROS_INFO("\033[1;33m ------------- \033[0m");
    }



    /***********************************************************
     * @brief X20足端位置数据记录成点云函数
     * 
     **********************************************************/
    void X20ContactPoseRecordProcess()
    {
        for (int i = 0; i < 4; i++)
        {
            if (X20Leg.foot_contact(i) > 0)
            {
                // 触地状态，还没进行记录足端位置数据处理程序段
                if (X20ContactPoseRecord(i) == false)
                {
                    /**** 将触地足端位置处理记录到PCL点云程序段 ****/
                    // 记录触地足端位置到点云中进行存储
                    pcl::PointXYZI thisContactPose;
                    thisContactPose.x = X20Leg.foot_pos_imu(0, i);
                    thisContactPose.y = X20Leg.foot_pos_imu(1, i);
                    thisContactPose.z = X20Leg.foot_pos_imu(2, i);
                    X20ContactPoseCloud->push_back(thisContactPose);
                    // ROS_INFO("\033[1;31m %ld; \033[0m", X20ContactPoseCloud->points.size());

                    X20ContactPoseRecord(i) = true; // 标记为已经进行足端触地位置处理了
                }
            }
            else
            {
                X20ContactPoseRecord(i) = false;    // 重新进行标记为未处理，等待触地状态
            }
        }
        // ROS_INFO("\033[1;33m ------------- \033[0m");
    }



    /***********************************************************
     * @brief 发布X20触地接触的足端位置点云函数
     * 
     **********************************************************/
    void publish_X20ContactPoseCloud()
    {
        sensor_msgs::PointCloud2 cloudMsgTemp;
        pcl::toROSMsg(*X20ContactPoseCloud, cloudMsgTemp);
        // cloudMsgTemp.header.stamp = ros::Time().fromSec(xx);
        cloudMsgTemp.header.stamp = ros::Time::now();
        cloudMsgTemp.header.frame_id = "camera_init";
        pubX20ContactPoseCloud.publish(cloudMsgTemp);

    }



    /***********************************************************
     * @brief 处理X20触地接触模块的主循环程序
     * 
     * 空循环
     **********************************************************/
    void processX20ContactModule()
    {
        ros::Rate rate(50);
        while (ros::ok())
        {
            publish_X20ContactPoseCloud();
            rate.sleep();
        }
    }


};


int main(int argc, char** argv)
{
    ros::init(argc, argv, "X20ContactModule");

    X20ContactModule MO;

    ROS_INFO("\033[1;32m----> X20ContactModule Procee Started.\033[0m");

    std::thread X20ContactModuleThread(&X20ContactModule::processX20ContactModule, &MO);

    ros::spin();

    X20ContactModuleThread.join();

    return 0;
}